#! /opt/local/bin/python2.6
import numpy as np
import time
import sys
from rwEXR import *


# 3744 23.9
# 5616 35.8


# distortion correction parameters in rad
# 0.207029529537 0.0422547753997


entrancePixel = (3341,5615-2345)

#entrancePixel = (1848,5615-2341)
physicCenter = (1867.5,5615-2979.5)
#center = (1884.18,2834.89)
#center = np.array([2836.5,3744-1860.79])

#center = (1853,5615-2874.5)
distortionCenter = (1860.79,2836.5)
#center = np.array([2836.5,3744-1860.79])


def Rd_distortion(pixel):
    c = distortionCenter
    return np.sqrt( (pixel[0] - c[0])*(pixel[0] - c[0]) + (pixel[1] - c[1])*(pixel[1] - c[1]) )

def Rd_physic(pixel):
    c = physicCenter
    return np.sqrt( (pixel[0] - c[0])*(pixel[0] - c[0]) + (pixel[1] - c[1])*(pixel[1] - c[1]) )


pix = np.sqrt(23.9*23.9 + 35.8*35.8)/np.sqrt(3744*3744+5616*5616)
print pix

coeffs = [ -1.54035227e-12 ,  6.94350405e-09  ,-1.22082544e-05,   9.47349416e-03,5.90440253e+00]
#coeffs = [ -1.94777642e-12 ,  9.17138801e-09 , -1.59443120e-05 ,  1.21270728e-02,5.15110092e+00]
coeffs = [ -1.48631764e-12  , 6.66110873e-09 , -1.16207010e-05 ,9.22816587e-03,5.78973689e+00]

poly_Rd2f = np.poly1d(coeffs)

f = poly_Rd2f(Rd_distortion(entrancePixel))

print f

#f = 8

d = 79+89

T1 = np.matrix([d*np.sin(82*np.pi/180.0),+d*np.cos(82*np.pi/180.0)-d,0])

#R = np.matrix([[0,-1,0],[1,0,0],[0,0,1]])

#e1 = np.matrix([-f,d+f,0]) 

#e2 = np.matrix([d+f,-f,0])

theta = 2*np.arcsin( Rd_distortion(entrancePixel)  / (2.0 * f/pix) )

phi   = np.arctan2( entrancePixel[1] - distortionCenter[1], entrancePixel[0] - distortionCenter[0] )

op1 = np.matrix([ np.sin(theta)*np.cos(phi), np.cos(theta), np.sin(theta)*np.sin(phi)])
print op1
XX = (entrancePixel[0] - distortionCenter[0]) / Rd_distortion(entrancePixel)*f * np.tan(2.0*np.arcsin(Rd_distortion(entrancePixel)/(2.0*f/pix))) + 7.5*pix
ZZ = (entrancePixel[1] - distortionCenter[1]) / Rd_distortion(entrancePixel)*f * np.tan(2.0*np.arcsin(Rd_distortion(entrancePixel)/(2.0*f/pix))) + 143*pix
op1 = np.matrix([XX,8,ZZ])
#E = np.matrix( [[0,0,-d],[0,0,-d],[d,-d,0]])
print (entrancePixel[0] - distortionCenter[0]) / Rd_distortion(entrancePixel)*f * np.tan(2.0*np.arcsin(Rd_distortion(entrancePixel)/(2.0*f/pix)))
N = np.cross(op1,T1) 

print N

#N = (op1 * E)

N = N.flat

n1 = N[0]
n2 = N[1]
n3 = N[2]

#n4 = -n1*(d+8) + n2*8

y = np.arange(-1000,1000,0.1)

z = (-n2*8-n2*y)/(n3)

ru = np.sqrt(z*z+y*y)

outPixel = (1621,5615-2415)
f = poly_Rd2f(Rd_distortion(outPixel))
print f


rd = 2*f*np.sin(np.arctan2(ru,f)/2)

U = -rd * np.cos(np.arctan2(z,y)) / pix
V = rd * np.sin(np.arctan2(z,y)) / pix

exrR,exrG,exrB,L,size = readExr("/Network/scratch/Tests/XL/DepthEstimationProject/sourceimages/HDRI/rotationFisheye/exr/vue1.exr")

for (u,v) in zip(U,V):
    
    L[u+distortionCenter[0]- 7.5*pix,v+distortionCenter[1]- 143*pix] = 255


createNewOutputImage("epipolar.exr",L.T,L.T,L.T,size)

print e1 * E 



















